//
// Created by HLiamso on 2022-03-31.
//

#include "Detect.h"
extern float t;
Device_t DEVICES[DEVICE_NUMBER+1];
void Detect_Init(){
    for(int i=0;i<DEVICE_NUMBER;i++){
        DEVICES[i].Device_Status=OK;
        DEVICES[i].Last_Response_TimeLine=0;
        DEVICES[i].No_Response_Time=0;
    }
    Tolerance_Time_Init();
}
void Detect_Task(){
    for(int i=0;i<DEVICE_NUMBER;i++){
        DEVICES[i].No_Response_Time=t-DEVICES[i].Last_Response_TimeLine;
        if(DEVICES[i].No_Response_Time>=DEVICES[i].Tolerance_Time){
            DEVICES[i].Device_Status=Not_OK;
        }
        if(DEVICES[i].No_Response_Time<DEVICES[i].Tolerance_Time){
            DEVICES[i].Device_Status=OK;
        }
    }
}
void Device_Activation(Device_List_t Device){
    DEVICES[Device].Last_Response_TimeLine=t;
}
void Tolerance_Time_Init(){
    DEVICES[IMU].Tolerance_Time=IMU_Tolerance_Time;
    DEVICES[DBUS].Tolerance_Time=BDUS_Tolerance_Time;
    DEVICES[YAW_MOTOR].Tolerance_Time=YAW_Tolerance_Time;
    DEVICES[PITCH_MOTOR].Tolerance_Time=PTICH_Tolerance_Time;
    DEVICES[CHASSIS_MOTOR].Tolerance_Time=CHASSIS_Tolerance_Time;
    DEVICES[FRICTION_MOTOR].Tolerance_Time=FRICTION_Tolerance_Time;
    DEVICES[CLIP_MOTOR].Tolerance_Time=CLIP_Tolerance_Time;
}
